classdef lqr_policy < handle 
    %UNTITLED Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        % model=[];
        inst_count_vec=[];
        rt_vec=[]; 
         m=[];
         m_inst_count_vec=[];
         m_rt_vec=[]; 
         samplingTime = 1;         
         lqr=[];
         x0=[];
    end
    
    methods
       function [m_x,d_x]=linearize(obj,x)
            m_x = mean(x);
            % d_x = x-m_x;
            d_x = x-ones(size(x,1),1)*m_x;
        end
        
        function d_x=linearize_around(obj,x,m_x)
            d_x = x-ones( size(x,1),1) * m_x ;
        end
        
        function x=delinearize_around(obj,d_x,m_x)
            x = m_x  + d_x;
        end
        
      
        function build_model(obj, inst_count_vec, rt_vec)
            obj.inst_count_vec=inst_count_vec;  
            obj.rt_vec=rt_vec;
            %----------
            n_st=1;% 1 state variable
            n_in= size(inst_count_vec,2);  %11 inputs
            n_out= size(rt_vec,2); % 1 output
            n_env=1; % environmental disturbances
            A=ones(n_st,n_st);
            B=ones(n_st,n_in);
            K=ones(n_st,n_env);
            C=ones(n_out,n_st);
            D=zeros(n_out,n_in);
            
            m_rt_vec=2;
            [  d_rt_vec]= obj.linearize_around(rt_vec , m_rt_vec);  %  2  seconds desired
            m_inst_count_vec=2;
            [ d_inst_count_vec] = obj.linearize_around(inst_count_vec,...
               ones(1, size(inst_count_vec,2))* m_inst_count_vec);
            
            u =  d_inst_count_vec;
            y = d_rt_vec;
            
            data=iddata(y, u , obj.samplingTime);
            m=idss(A,B,C,D,K,'Ts', obj.samplingTime,'DisturbanceModel','Estimate','InitialState','Estimate');
            m1=init(m);
            mm=pem(data,m1);

            obj.m=mm;
            obj.m_inst_count_vec=m_inst_count_vec; 
            obj.m_rt_vec = m_rt_vec; 
            
            %-------------------------------------------------
            % Design LQ-optimal gain K
%            K = lqry(sys,10,1)  % u = -Kx minimizes J(u)
            relativeCost=1000;
            Q=relativeCost*eye(1); 
            R=diag(prices_amz().get_hourly_price); %eye(11);
            N=zeros(1,11); 
            sys = ss(mm,'m');
            sys_noise = ss(mm);
            % P = sys_noise(:,[1 1]);
            % [K,S,e] = lqr(mm.A,mm.B,Q,R,N)  doesnt consider noise
            % [K,S,e] = lqr(sys,Q,R,N) 
            % [K,S,e] = lqrd(sys.a,sys.b,Q,R,1);  -----
             [K,S,e] =  lqry(sys,Q,R,N)
            % Design Kalman state estimator Kest.
            % noise covariance data of the plant  
%             Qk=eye(11);
%             Rk=eye(1);
%             Nk=eye(11);
           %  Kest = kalman(sys,Qk,Rk,Nk,[1 1 1 1 1 1 1 1 1 1 1], [1])
            % Kest = kalman(sys_noise,1,0.01)
            [Kest,L,P] = kalman(sys_noise,1,0.01); 
            
            % Form LQG regulator = LQ gain + Kalman filter.
            F = lqgreg(Kest,K);
            obj.lqr=F;
            
            clsys = feedback(sys_noise,F,+1);
            %---------------------------------- way 1
            noise=randn(60,2)*0.001; 
            x_sys = mm.X0; % system state  for the first time
            rt_acc=[];
            u_opt_acc=[];
            x_ctrl=0;
            for i=1:60                
                rt_ = rt_vec(end,:);  % system output - kalman input for the first time                
                u_opt=F.c*x_ctrl + F.d * rt_;
                x_ctrl=F.a* x_ctrl + F.b * rt_;
                %------ system update ------
                rt_=sys_noise.c*x_sys + sys_noise.d*[u_opt ; noise(i,1)];
                x_sys =sys_noise.a*x_sys + sys_noise.b*[u_opt ; noise(i,1)];
                rt_acc=[rt_acc rt_];
                u_opt_acc=[u_opt_acc ; u_opt'];
            end
            rt_acc = obj.delinearize_around(rt_acc,m_rt_vec); 
            plot(rt_acc); 

            %---------------------------------- way 2
            %      inp=rand
            noise=randn(60,2);
            
            x_kalm = mm.X0 % kalman state for the first time
            x_sys = mm.X0; % system state  for the first time
            x_lqr=0;
            
            rt_ = rt_vec(end,:);  % system output - kalman input for the first time
            u_opt = inst_count_vec(end,:)';  % controller output-kalman input for the first time
            
            
            rt_acc=[];            
            u_opt_acc=[]; 
            for i=1:60
                %------  kalman update ------
                tmp=Kest.c*x_kalm + Kest.d*[u_opt ; rt_]; 
                y1_e=tmp(1);
                x1_e = tmp(2); 
                x_kalm =Kest.a*x_kalm + Kest.b*[u_opt ; rt_]; 
                                
                %------ controller update ------
                 u_opt =-1*K*x1_e;
                 x_lqr=(Kest.a-L*Kest.c-(Kest.B-L*Kest.D)*K)*x1_e+L*rt_;
                % u_opt = [1 0 0 0 0 0 0 0 0 0 0]'; 
                
                %------ system update ------
                rt_=sys_noise.c*x_sys + sys_noise.d*[u_opt ; noise(i,1)];
                x_sys =sys_noise.a*x_sys + sys_noise.b*[u_opt ; noise(i,1)]; 
                rt_acc=[rt_acc rt_]; 
                u_opt_acc=[u_opt_acc ; u_opt'];
            end
        %    clsys = feedback(sys,F,+1);
            rt_acc = obj.delinearize_around(rt_acc,m_rt_vec); 
            plot(rt_acc); 
            
        
        end        
        
        function obj=lqr_policy()
%          %   obj.model=model;                        
% 
%             % Design LQ-optimal gain K
%             K = lqry(sys,10,1)  % u = -Kx minimizes J(u)
% 
%             [K,S,e] = lqr(SYS,Q,R,N) 
% 
%             % Separate control input u and disturbance input d
%             P = sys(:,[1 1]);
%             % input [u;d], output y
% 
%             % Design Kalman state estimator Kest.
%             Kest = kalman(P,1,0.01)
% 
%             % Form LQG regulator = LQ gain + Kalman filter.
%             F = lqgreg(Kest,K)
% 
%             % Close loop
%             clsys = feedback(sys,F,+1)
%             % Note positive feedback.

        end
        
        function [instance_quantity_to_fire,instance_quantity_to_destroy]=get_choice_at_time(obj,t)
            default=[0 0 0 0 0 0 0 0 0 0 0];
            % min, type, num
            instance_quantity_to_fire=default;  
            instance_quantity_to_destroy=default;  
            t=[0,1];
            x0=obj.lqr;
            lsim(obj.lqr,[],t,x0)
            
            obj.lqr.a*1+obj.lqr.b*ones(12,1)
            obj.lqr.c*1+obj.lqr.d*2.4
%             instance_quantity_to_fire(type)=num;
%             instance_quantity_to_destroy(type)=num;
                        
        end

    end
    
end

